#include "img_fuse.h"

FuseImg::FuseImg()
{
    subPointCloud = nh.subscribe<sensor_msgs::PointCloud2>("rslidar_points", 1, &FuseImg::PointCallback, this);
    subImg = nh.subscribe<sensor_msgs::Image>("camera_image",1,&FuseImg::ImgCallback, this);

    pubFuseImg = nh.advertise<sensor_msgs::Image>("fuse_img", 1);

    transfromEigentoCamera<<   2.4218716644699434e-03, -9.9999449089368175e-01, -2.2699603360241682e-03,  5.4486728759709918e-02,
                              -8.5947329009445905e-03,  2.2490678031144018e-03, -9.9996053535146001e-01, -2.0519654677992365e-01,
                               9.9996013175726284e-01,  2.4412857889399286e-03, -8.5892385980634867e-03, -2.0146010826086281e-01,
                                                    0,                       0,                       0,                       1;

    cameraPara            <<   1.8666088127172050e+03,                       0,  6.1897171513522983e+02,                       0,
                                                    0,  1.9552563369016768e+03,  5.1613593890827763e+02,                       0,
                                                    0,                       0,                       1,                       0,
                                                    0,                       0,                       0,                       1;
}

void FuseImg::PointCallback(const sensor_msgs::PointCloud2ConstPtr& ptmsg)
{
	pcl::PointCloud<PointType>::Ptr pcl_cloud(new pcl::PointCloud<PointType>);
    pcl::fromROSMsg(*ptmsg, *pcl_cloud);
	pcl_cloud_ =* pcl_cloud;
}

void FuseImg::ImgCallback(const sensor_msgs::ImageConstPtr &msgImg)
{
    cv_bridge::CvImagePtr cvImgPtr;
    try{
        cvImgPtr=cv_bridge::toCvCopy(msgImg,sensor_msgs::image_encodings::BGR8);
    }
    catch(cv_bridge::Exception e){
        ROS_ERROR_STREAM("Cv_bridge Exception:"<<e.what());
        return;
    }
    cvColorImgMat=cvImgPtr->image;

    for(size_t i = 0; i < pcl_cloud_.size(); i++)
	{
        if (pcl_cloud_.points[i].x > 0)
        {
            thispoint << pcl_cloud_.points[i].x, pcl_cloud_.points[i].y, pcl_cloud_.points[i].z, 1;

            thispoint = cameraPara*(transfromEigentoCamera*thispoint);
            thispoint /= thispoint[2];

            cv::Scalar PointColor = fakecolor(pcl_cloud_.points[i].intensity);
            if((0<thispoint[0])&&(thispoint[0]<2000)&&(0<thispoint[1])&&(thispoint[1]<2000))
            {
                cv::circle(cvColorImgMat, cv::Point (thispoint[0], thispoint[1]), 2, PointColor);
            }
        }
	}

    fusemsg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", cvColorImgMat).toImageMsg();
    pubFuseImg.publish(fusemsg);
    ROS_INFO("Image PCL fused.");
}

cv::Scalar fakecolor(int intensity){
    int fakecolor = intensity*2.55*6;
    if (fakecolor <= 255)
    {
        return cv::Scalar (fakecolor, 0, 0);
    }
    else if (fakecolor <= 510)
    {
        return cv::Scalar (255, fakecolor-255, 0);
    }
    else if (fakecolor <= 765)
    {
        return cv::Scalar (255, 255, fakecolor-510);
    }
    else if (fakecolor <= 1020)
    {
        return cv::Scalar (fakecolor-765, 255, 255);
    }
    else if (fakecolor <= 1275)
    {
        return cv::Scalar (0, fakecolor-1020, 255);
    }
    else if (fakecolor <= 1530)
    {
        return cv::Scalar (0, 0, fakecolor-1275);
    }
}